#include "tracker.h"
#include "../utils/utils.h"
#include "phv_filter.h"

#include <utils/utils.h>

#include <chrono>
#include <iostream>
#include <list>

namespace fusion_perception {

Tracker::Tracker(int64_t id)
    : id_(id)
{
    phvFilterSptr_ = std::make_shared<PHVFilter>();

    history_.emplace_front(std::make_shared<Object>(), ObjectSptrVector {}, ObjectSptrVector {});
}

ObjectSptr Tracker::get(void)
{
    return std::get<0>(history_.front());
}

int64_t Tracker::getId(void) const
{
    return id_;
}

ObjectSptr Tracker::operator->(void)
{
    return get();
}

Object& Tracker::operator*(void)
{
    return *get();
}

void Tracker::fuse(int64_t microTs, const ObjectSptrVector& objectSptrVector)
{
    auto objectSptr = std::make_shared<Object>(*this->get());

    ObjectSptrVector cameraObjectSptrVector;
    ObjectSptrVector radarObjectSptrVector;

    for (auto& i : objectSptrVector) {
        switch (i->source) {
        case 1: // camera
            cameraObjectSptrVector.emplace_back(i);
            break;
        case 2: // radar
            radarObjectSptrVector.emplace_back(i);
        default:
            break;
        }
    }

    fuseHeader(microTs, *objectSptr, cameraObjectSptrVector, radarObjectSptrVector);
    fuseId(microTs, *objectSptr, cameraObjectSptrVector, radarObjectSptrVector);
    fuseType(microTs, *objectSptr, cameraObjectSptrVector, radarObjectSptrVector);
    fuseSize(microTs, *objectSptr, cameraObjectSptrVector, radarObjectSptrVector);
    fusePHV(microTs, *objectSptr, cameraObjectSptrVector, radarObjectSptrVector);
    fuseColor(microTs, *objectSptr, cameraObjectSptrVector, radarObjectSptrVector);
    fusePlate(microTs, *objectSptr, cameraObjectSptrVector, radarObjectSptrVector);
    fusePlateColor(microTs, *objectSptr, cameraObjectSptrVector, radarObjectSptrVector);
    fuseSource(microTs, *objectSptr, cameraObjectSptrVector, radarObjectSptrVector);
    fuseFeature(microTs, *objectSptr, cameraObjectSptrVector, radarObjectSptrVector);

    if (history_.size() >= 100) {
        history_.pop_back();
    }

    history_.emplace_front(objectSptr, cameraObjectSptrVector, radarObjectSptrVector);
}

int32_t Tracker::getStatus(void) const
{
    return status_;
}

std::deque<std::tuple<ObjectSptr, ObjectSptrVector, ObjectSptrVector>> Tracker::getHistory(void) const
{
    return history_;
}

void Tracker::setStatus(int32_t status)
{
    status_ = status;
}

void Tracker::fuseHeader(int64_t microTs, Object& object, const ObjectSptrVector&, const ObjectSptrVector&)
{
    object.header.frame_id = "multiFused";
    object.header.stamp.sec = microTs / 1000000;
    object.header.stamp.nanosec = (microTs % 1000000) * 1000;
}

void Tracker::fuseId(int64_t, Object& object, const ObjectSptrVector&, const ObjectSptrVector&)
{
    object.id = getId();
}

void Tracker::fuseSource(int64_t, Object& object, const ObjectSptrVector&, const ObjectSptrVector&)
{
    if (getStatus() > 0) {
        object.source = 102; // perdicted
    } else {
        object.source = 101; // fused
    }
}

double Tracker::calculateHeading(const std::list<double>& newHeadings)
{
    // 得到最后一次融合结果
    auto prevObject = get();

    // 如果这个tracker比较"新",那么基本没有历史数据做参考
    if (history_.size() < 3) {
        double heading = prevObject->heading;

        if (!newHeadings.empty()) {
            heading = utils::angleAverage(newHeadings);
        }

        // 剔除新值中异常航向角
        if (newHeadings.size() >= 3) {
            std::list<double> newAnglesTmp;
            for (auto& i : newHeadings) {
                if (std::abs(utils::angleDiff(i, heading)) <= 60) {
                    newAnglesTmp.emplace_back(i);
                }
            }

            if (!newAnglesTmp.empty()) {
                heading = utils::angleAverage(newAnglesTmp);
            }
        }

        return heading;
    }

    size_t i = 20;

    // 计算历史融合结果的航向角平均值
    std::list<double> historyHeadings;
    double historyHeadingAverage = prevObject->heading;

    for (auto begin = history_.begin(); begin != history_.end() && i > 0; begin++, i--) {
        auto object = std::get<0>(history_.front());
        historyHeadings.emplace_back(object->heading);
    }

    if (!historyHeadings.empty()) {
        historyHeadingAverage = utils::angleAverage(historyHeadings);
    }

    // 计算本次融合匹配到的传感器数据航向角的平均值
    std::list<double> headings;
    auto heading = prevObject->heading;

    for (auto& i : newHeadings) {
        if (std::abs(utils::angleDiff(i, historyHeadingAverage)) <= 10) {
            headings.emplace_back(i);
        }
    }

    if (!headings.empty()) {
        heading = utils::angleAverage(headings);
    }

    return heading;
}

void Tracker::fusePHV(int64_t microTs, Object& object, const ObjectSptrVector& cameraObjectSptrVector, const ObjectSptrVector& radarObjectSptrVector)
{
    PHVFilter::MeasuredPosition measuredPosition;

    ObjectSptrVector objectSptrVector;
    objectSptrVector.insert(objectSptrVector.end(), cameraObjectSptrVector.begin(), cameraObjectSptrVector.end());
    objectSptrVector.insert(objectSptrVector.end(), radarObjectSptrVector.begin(), radarObjectSptrVector.end());

    int64_t ts = 0;

    std::list<double> angles;
    if (!objectSptrVector.empty()) {
        setStatus(0);

        for (auto& i : objectSptrVector) {
            ts += (int64_t)i->header.stamp.sec * 1000000 + i->header.stamp.nanosec / 1000;
            measuredPosition.x += i->x;
            measuredPosition.y += i->y;
            angles.emplace_back(i->heading);
            measuredPosition.vx += i->speed * sin(i->heading * M_PI / 180);
            measuredPosition.vy += i->speed * cos(i->heading * M_PI / 180);
        }

        ts = ts / objectSptrVector.size();
        measuredPosition.x /= objectSptrVector.size();
        measuredPosition.y /= objectSptrVector.size();
        measuredPosition.vx /= objectSptrVector.size();
        measuredPosition.vy /= objectSptrVector.size();
        measuredPosition.heading = calculateHeading(angles);
    } else {
        setStatus(getStatus() + 1);

        ts = (int64_t)object.header.stamp.sec * 1000000 + object.header.stamp.nanosec / 1000;
        measuredPosition.heading = object.heading;
        measuredPosition.x = object.x;
        measuredPosition.y = object.y;
        measuredPosition.vx = object.speed * sin(object.heading * M_PI / 180);
        measuredPosition.vy = object.speed * cos(object.heading * M_PI / 180);
    }

    auto dtS = double(microTs - ts) / 1000000;

    dtS = 0;

    auto heading = measuredPosition.heading;

    measuredPosition.heading = sin(measuredPosition.heading * M_PI / 180.0);

    auto correctedPosition = phvFilterSptr_->correct(dtS, measuredPosition);

    heading = fmod((atan2(correctedPosition.heading, cos(heading * M_PI / 180.0)) * 180.0 / M_PI + 360.0), 360.0);

    object.x = correctedPosition.x;
    object.y = correctedPosition.y;
    object.heading = heading;
    object.speedx = correctedPosition.vx;
    object.speedy = correctedPosition.vy;
}

void Tracker::fuseSize(int64_t, Object& object, const ObjectSptrVector& cameraObjectSptrVector, const ObjectSptrVector&)
{
    if (cameraObjectSptrVector.empty()) {
        return;
    }

    double length = 0.0;
    double width = 0.0;
    double height = 0.0;

    for (auto& i : cameraObjectSptrVector) {
        length += i->length;
        width += i->width;
        height += i->height;
    }

    length /= cameraObjectSptrVector.size();
    width /= cameraObjectSptrVector.size();
    height /= cameraObjectSptrVector.size();

    object.length = length;
    object.width = width;
    object.height = height;
}

void Tracker::fuseType(int64_t, Object& object, const ObjectSptrVector& cameraObjectSptrVector, const ObjectSptrVector&)
{
    for (auto& i : cameraObjectSptrVector) {
        typeStatistician_[i->type]++;
    }

    int32_t min = 0;
    int32_t type = -1;

    for (auto& i : typeStatistician_) {
        if (i.second > min) {
            min = i.second;
            type = i.first;
        }
    }

    if (type != -1) {
        object.type = type;
    }
}

void Tracker::fuseColor(int64_t, Object& object, const ObjectSptrVector& cameraObjectSptrVector, const ObjectSptrVector&)
{
    for (auto& i : cameraObjectSptrVector) {
        colorStatistician_[i->extend.color]++;
    }

    int32_t min = 0;
    std::string color;

    for (auto& i : colorStatistician_) {
        if (i.second > min) {
            min = i.second;
            color = i.first;
        }
    }

    if (!color.empty()) {
        object.extend.color = color;
    }
}

void Tracker::fusePlate(int64_t, Object& object, const ObjectSptrVector& cameraObjectSptrVector, const ObjectSptrVector&)
{
    for (auto& i : cameraObjectSptrVector) {
        plateStatistician_[i->extend.plate]++;
    }

    int32_t min = 0;
    std::string plate;

    for (auto& i : plateStatistician_) {
        if (i.second > min) {
            min = i.second;
            plate = i.first;
        }
    }

    if (!plate.empty()) {
        object.extend.plate = plate;
    }
}

void Tracker::fusePlateColor(int64_t, Object& object, const ObjectSptrVector& cameraObjectSptrVector, const ObjectSptrVector&)
{
    for (auto& i : cameraObjectSptrVector) {
        plateColorStatistician_[i->extend.plate_color]++;
    }

    int32_t min = 0;
    std::string plateColor;

    for (auto& i : colorStatistician_) {
        if (i.second > min) {
            min = i.second;
            plateColor = i.first;
        }
    }

    if (!plateColor.empty()) {
        object.extend.plate_color = plateColor;
    }
}

void Tracker::fuseFeature(int64_t, Object& object, const ObjectSptrVector& cameraObjectSptrVector, const ObjectSptrVector&)
{
    if (cameraObjectSptrVector.empty()) {
        object.extend.feature.clear();
        return;
    }

    if (object.extend.feature.empty()) {
        object.extend.feature = cameraObjectSptrVector.at(0)->extend.feature;
        return;
    }

    auto feature = object.extend.feature;
    int32_t score = INT32_MAX; //[-1,1],高于0.3为不准,值越低越准

    for (auto& i : cameraObjectSptrVector) {
        auto curScore = cosineDist(object.extend.feature, i->extend.feature);
        if (curScore < score) {
            feature = i->extend.feature;
            score = curScore;
        }
    }

    if (score > 0.3) {
        object.extend.feature.clear();
    } else {
        object.extend.feature = feature;
    }
}

}